import rospy
import rospkg
from gazebo_msgs.msg import ModelState
from gazebo_msgs.srv import SetModelState
import math
import time
mode='straight'
mode='ellipse'
#mode='static'
def main():
    rospy.init_node('set_pose')

    # 椭圆参数
    cx=-1.8
    cy=0.4
    a = 2.4  # 长半轴
    b = 1.85  # 短半轴
    omega = 2*math.pi/60  # 角速度

    vel=0.3

    state_msg = ModelState()
    state_msg.model_name = 'm2wr'

    rate = rospy.Rate(100)

    t0=time.time()

    while not rospy.is_shutdown():
        t1 = time.time()
        t=(t1-t0)
        # 椭圆轨迹的参数方程
        print(t)

        x=0
        y=0
        z=0.1
        if mode=='ellipse':
            x = a * math.cos(omega * t)+cx
            y = b * math.sin(omega * t)+cy
            z = 0.1
        if mode=='straight':
            x = vel*t
            y = 0
            z = 0.1

        state_msg.pose.position.x = x
        state_msg.pose.position.y = y
        state_msg.pose.position.z = z
        state_msg.pose.orientation.x = 0
        state_msg.pose.orientation.y = 0
        state_msg.pose.orientation.z = 0
        state_msg.pose.orientation.w = 1

        rospy.wait_for_service('/gazebo/set_model_state')
        try:
            set_state = rospy.ServiceProxy('/gazebo/set_model_state', SetModelState)
            resp = set_state(state_msg)
        except rospy.ServiceException as e:
            print("Service call failed: {}".format(e))
        rate.sleep()


if __name__ == '__main__':
    try:
        main()
    except rospy.ROSInterruptException:
        pass


